
#include <VirtualRobot/RuntimeEnvironment.h>
#include <VirtualRobot/Import/RobotImporterFactory.h>

#include <Inventor/Qt/SoQt.h>
#include <QFileInfo>

#include <boost/extension/shared_library.hpp>
#include <boost/function.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/filesystem.hpp>
#include <boost/foreach.hpp>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string.hpp>
#include <string>
#include <map>
#include <iostream>
#include <fstream>
#include <sstream>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <MMM/Motion/MotionReaderXML.h>
#include <MMM/Motion/MotionReaderC3D.h>
#include <MMM/Model/ModelReaderXML.h>
#include <MMM/Model/ModelProcessor.h>
#include <MMM/Model/ModelProcessorFactory.h>
#include <MMM/Motion/Legacy/Converter/ConverterFactory.h>

#include <VirtualRobot/VirtualRobot.h>
#include <MMMSimoxTools/MMMSimoxTools.h>
#include <VirtualRobot/RobotNodeSet.h>



using std::cout;
using std::endl;
using namespace VirtualRobot;

int main(int argc, char *argv[])
{
    cout << " --- MMM get6dtraj --- " << endl;
    cout << "get trajectory for a robot node/joint in the first motion from a motion file" << endl;

    // we are expecting three arguments, and that is the filename of the file to convert
    if (argc<4)
    {
        cout << "Missing filename as first argument, aborting..." << endl;
        cout << "Syntax: get6dtraj %motionfile_in %robotnode/jointname %outputfile (%mode)" << endl;
        //MMM_ERROR << endl << "Could not process command line, aborting..." << endl;
        return -1;
    }
    //c.print();


    std::string filename = std::string(argv[1]);
    std::string nodename = std::string(argv[2]);
    std::string filenameOut = std::string(argv[3]);

    std::string mode = "rpy";
    if(argc == 5)
    {
        mode = std::string(argv[4]);
    }

    cout << "Filenames provided: [" << filename << "] -> [" << filenameOut << "]" << endl;

    if (!filename.empty())
    {
        // input file 1
        QFileInfo fileInfo(filename.c_str());
        std::string suffix(fileInfo.suffix().toAscii());

        // transform suffixes to lowerCase
        std::transform(suffix.begin(), suffix.end(), suffix.begin(), ::tolower);

        // For now, we just concatenate MMM motion files, so we check for correct suffixes
        if ((suffix.compare("xml")!=0))
        {
            cout << "Expected MMM Motion files as input (.xml). Aborting..." << endl;
            return -1;
        }
        // load input files
        MMM::MotionReaderXMLPtr r(new MMM::MotionReaderXML());
        cout << "Checking motions in file: " << endl;

        MMM::MotionPtr motion = r->loadMotion(filename);
        MMM::ModelPtr model = motion->getModel();

        VirtualRobot::RobotPtr robot = MMM::SimoxTools::buildModel(model);
        MMM::SimoxTools::updateInertialMatricesFromModels(robot);

        std::vector<std::string> jointNames = motion->getJointNames();
        VirtualRobot::RobotNodeSetPtr robotNodeSet = VirtualRobot::RobotNodeSet::createRobotNodeSet(robot, "VNodeSet", jointNames, "", "", true);

        if(filenameOut.empty())
        {
            filenameOut = "output.csv";
        }

        std::ofstream ofs(filenameOut.c_str());

        for(size_t i = 0; i < motion->getNumFrames(); ++i)
        {
            MMM::MotionFramePtr frame = motion->getMotionFrame(i);
            robot->setGlobalPose(frame->getRootPose());
            robotNodeSet->setJointValues(frame->joint);

            Eigen::Matrix4f pose = robot->getRobotNode(nodename)->getPoseInRootFrame();


            //write into file
            ofs << frame->timestep << ",";
            ofs << pose(0,3) << "," << pose(1,3) << "," << pose(2,3) << ",";

            if(mode == "rpy")
            {
                Eigen::Vector3f rpy;
                VirtualRobot::MathTools::eigen4f2rpy(pose,rpy);
                ofs << rpy(0) << "," << rpy(1) << "," << rpy(2);
            }
            else if(mode == "quat")
            {
                VirtualRobot::MathTools::Quaternion q = VirtualRobot::MathTools::eigen4f2quat(pose);
                ofs << q.w << "," << q.x << "," << q.y << "," << q.z;
            }
            ofs << '\n';
        }

        ofs.close();

    }
    return 0;
}
